﻿#include "AircraftSimulation.h"
using namespace std;
using namespace ASL;

// 构造函数
AutoPilot::AutoPilot()
{
    input.time = 0.0;
    input.LLA.init(3);
    input.vNav.init(3);
    input.eular.init(3);
    input.omega.init(3);
    input.groundReaction = false;

    output.aileronCommand = 0.0;
    output.brakeCommand = 0.0;
    output.elevatorCommand = 0.0;
    output.gearCommand = 0;
    output.rudderCommand = 0.0;
    output.speedBrakeCommand = 0.0;
    output.steerCommand = 0.0;
    output.throttleCommand = 0.0;
}

// 析构函数
AutoPilot::~AutoPilot()
{
}

// 初始化函数
bool AutoPilot::initialize(void)
{
    output.aileronCommand = 0.0;
    output.brakeCommand = 0.0;
    output.elevatorCommand = 0.0;
    output.gearCommand = 1;
    output.rudderCommand = 0.0;
    output.speedBrakeCommand = 0.0;
    output.steerCommand = 0.0;
    output.throttleCommand = 0.0;

    vPhase = AutoPilot::TakeOff;
    vTime = 0.0;

    return true;
}

// 单步执行函数
bool AutoPilot::step(void)
{
    if (vPhase == AutoPilot::TakeOff && input.time >= 120.0)
    {
        vPhase = AutoPilot::Cruise;
    }
    else if (vPhase == AutoPilot::Cruise && input.time >= 180.0)
    {
        vPhase = AutoPilot::Landing;
        vTime = input.time;
    }

    if (vPhase == AutoPilot::TakeOff)
    {
        this->takeOff();
    }
    else if (vPhase == AutoPilot::Cruise)
    {
        this->cruise();
    }
    else if (vPhase == AutoPilot::Landing)
    {
        this->landing();
    }

    return true;
}

// 程序控制算法
void AutoPilot::takeOff(void)
{
    double v;

    if (input.time < 5.0)
    {
        output.elevatorCommand = 0.0;
        output.throttleCommand = 0.0;
    }
    else if (input.time < 15.0)
    {
        output.elevatorCommand = 0.0;
        output.throttleCommand = 0.0 + 0.25 * (input.time - 5.0);
        output.throttleCommand = Basic::limit(output.throttleCommand, 0.0, 0.5);
    }
    else if (input.time < 22.5)
    {
        output.elevatorCommand = -0.05;
        output.throttleCommand = 0.5;
    }
    else if (input.time < 24.5)
    {
        output.elevatorCommand = -0.05;
        output.throttleCommand = 0.5 + 0.25 * (input.time - 22.5);
        output.throttleCommand = Basic::limit(output.throttleCommand, 0.0, 1.0);
    }
    else
    {
        output.elevatorCommand = -0.05;
        output.throttleCommand = 1.0;
    }

    if (input.LLA(AXIS_ALTITUDE) >= 100.0)
    {
        output.steerCommand = 0.0;
        output.gearCommand = 0;

        output.aileronCommand = 0.3 * (input.eular(AXIS_X) - 0.0) + 0.02 * (input.omega(AXIS_X) - 0.0);
        output.aileronCommand = Basic::limit(-output.aileronCommand / 21.5, -1.0, 1.0);

        output.elevatorCommand = 0.25 * (input.eular(AXIS_Z) - 6.0) + 0.02 * (input.omega(AXIS_Z) - 0.0);
        output.elevatorCommand = Basic::limit(output.elevatorCommand / 25.0, -1.0, 1.0);
    }

    if (input.LLA(AXIS_ALTITUDE) >= 300.0)
    {
        v = 0.06 * (input.LLA(AXIS_ALTITUDE) - 300.0) + 0.03 * (input.vNav(AXIS_U) - 0.0);
        output.elevatorCommand += v / 25.0;
        output.elevatorCommand = Basic::limit(output.elevatorCommand, -1.0, 1.0);
    }

    v = Vector::norm(input.vNav);

    if (v >= 200.0)
    {
        output.throttleCommand = 0.5 - 0.01 * (v - 250.0);
        output.throttleCommand = Basic::limit(output.throttleCommand, 0.0, 1.0);
    }
}

// 程序控制算法
void AutoPilot::cruise(void)
{
    double v;

    v = Vector::norm(input.vNav);

    output.throttleCommand = 0.5 - 0.01 * (v - 250.0);
    output.throttleCommand = Basic::limit(output.throttleCommand, 0.0, 1.0);

    output.aileronCommand = 0.3 * (input.eular(AXIS_X) - 0.0) + 0.02 * (input.omega(AXIS_X) - 0.0);
    output.aileronCommand = Basic::limit(-output.aileronCommand / 21.5, -1.0, 1.0);

    output.elevatorCommand = 0.25 * (input.eular(AXIS_Z) - 3.0) + 0.02 * (input.omega(AXIS_Z) - 0.0);
    output.elevatorCommand = Basic::limit(output.elevatorCommand / 25.0, -1.0, 1.0);

    v = 0.06 * (input.LLA(AXIS_ALTITUDE) - 300.0) + 0.03 * (input.vNav(AXIS_U) - 0.0);
    output.elevatorCommand += v / 25.0;
    output.elevatorCommand = Basic::limit(output.elevatorCommand, -1.0, 1.0);

    output.rudderCommand = 0.15 * (input.eular(AXIS_Y) - 30.0) + 0.01 * (input.omega(AXIS_Y) - 0.0);
    output.rudderCommand = Basic::limit(-output.rudderCommand / 30.0, -1.0, 1.0);
}

// 程序控制算法
void AutoPilot::landing(void)
{
    double v;
    double cV, cH, dT = 20.0;

    output.throttleCommand = 0.1;

    output.aileronCommand = 0.3 * (input.eular(AXIS_X) + 3.0) + 0.02 * (input.omega(AXIS_X) - 0.0);
    output.aileronCommand = Basic::limit(-output.aileronCommand / 21.5, -1.0, 1.0);

    output.rudderCommand = 0.0;

    cH = 300.0 * exp(-(input.time - vTime) / dT);
    cV = 300.0 * exp(-(input.time - vTime) / dT) * (-1.0 / dT);

    output.elevatorCommand = 0.25 * (input.eular(AXIS_Z) - 0.0) + 0.02 * (input.omega(AXIS_Z) - 0.0);
    output.elevatorCommand = Basic::limit(output.elevatorCommand / 25.0, -1.0, 1.0);

    v = 0.15 * (input.LLA(AXIS_ALTITUDE) - cH) + 0.035 * (input.vNav(AXIS_U) - cV);
    output.elevatorCommand += v / 25.0;
    output.elevatorCommand = Basic::limit(output.elevatorCommand, -1.0, 1.0);

    if (input.LLA(AXIS_ALTITUDE) < 100.0)
    {
        output.steerCommand = 0.0;
        output.gearCommand = 1;
    }

    if (input.groundReaction && input.time >= 250.0)
    {
        output.speedBrakeCommand = 1;
    }

    v = Vector::norm(input.vNav);

    if (input.time >= 250.0)
    {
        output.throttleCommand = 0.0;
        output.brakeCommand = 0.6;
        output.steerCommand = 0.0;
    }

    if (input.time >= 300.0)
    {
        output.throttleCommand = 0.0;
        output.brakeCommand = 0.0;
        output.steerCommand = -0.02;
    }

    if (input.time >= 320.0)
    {
        output.throttleCommand = 0.0;
        output.brakeCommand = 0.1;
        output.steerCommand = 0.0;
    }
}

// 构造函数
AircraftSimulation::AircraftSimulation()
{
    startTime = 0;
    stopTime = 0;
    stepSize = 0;
    currentTime = 0;
    currentCount = 0;
}

// 析构函数
AircraftSimulation::~AircraftSimulation()
{
}

// 准备函数
bool AircraftSimulation::setup(void)
{
    // 仿真求解配置
    startTime = 0.0;
    stopTime = 350.0;
    stepSize = 0.005;
    currentTime = 0.0;
    currentCount = 0;

    // 六自由度运动学模型参数
    cDOFModel.parameter.LLA(0) = 122.0;
    cDOFModel.parameter.LLA(1) = 47.0;
    cDOFModel.parameter.LLA(2) = 1.6764;

    cDOFModel.parameter.vBody(0) = 0;
    cDOFModel.parameter.vBody(1) = 0;
    cDOFModel.parameter.vBody(2) = 0;

    cDOFModel.parameter.eular(0) = 0;
    cDOFModel.parameter.eular(1) = 0;
    cDOFModel.parameter.eular(2) = 0;

    cDOFModel.parameter.omega(0) = 0;
    cDOFModel.parameter.omega(1) = 0;
    cDOFModel.parameter.omega(2) = 0;

    // 副翼执行机构参数
    cAileronActutor.parameter.initialValue = 0.0;
    cAileronActutor.parameter.k = 1.0 / 0.0495;
    cAileronActutor.parameter.locationMin = -21.5;
    cAileronActutor.parameter.locationMax = 21.5;
    cAileronActutor.parameter.velocityMin = -80.0;
    cAileronActutor.parameter.velocityMax = 80.0;

    // 方向舵执行机构参数
    cRudderActutor.parameter.initialValue = 0.0;
    cRudderActutor.parameter.k = 1.0 / 0.0495;
    cRudderActutor.parameter.locationMin = -30.0;
    cRudderActutor.parameter.locationMax = 30.0;
    cRudderActutor.parameter.velocityMin = -120.0;
    cRudderActutor.parameter.velocityMax = 120.0;

    // 升降舵执行机构参数
    cElevatorActutor.parameter.initialValue = 0.0;
    cElevatorActutor.parameter.k = 1.0 / 0.0495;
    cElevatorActutor.parameter.locationMin = -25.0;
    cElevatorActutor.parameter.locationMax = 25.0;
    cElevatorActutor.parameter.velocityMin = -60.0;
    cElevatorActutor.parameter.velocityMax = 60.0;

    // 前缘襟翼执行机构参数
    cLeadingActutor.parameter.initialValue = 0.0;
    cLeadingActutor.parameter.k = 1.0 / 0.136;
    cLeadingActutor.parameter.locationMin = -2.0;
    cLeadingActutor.parameter.locationMax = 25.0;
    cLeadingActutor.parameter.velocityMin = -25.0;
    cLeadingActutor.parameter.velocityMax = 25.0;

    // 后缘襟翼执行机构参数
    cTrailingActutor.parameter.initialValue = 0.0;
    cTrailingActutor.parameter.k = 1.0 / 0.136;
    cTrailingActutor.parameter.locationMin = -2.0;
    cTrailingActutor.parameter.locationMax = 20.0;
    cTrailingActutor.parameter.velocityMin = -25.0;
    cTrailingActutor.parameter.velocityMax = 25.0;

    // 减速板执行机构参数
    cSpeedActutor.parameter.initialValue = 0.0;
    cSpeedActutor.parameter.k = 1.0 / 0.136;
    cSpeedActutor.parameter.locationMin = 0.0;
    cSpeedActutor.parameter.locationMax = 60.0;
    cSpeedActutor.parameter.velocityMin = -25.0;
    cSpeedActutor.parameter.velocityMax = 25.0;

    // 创建本地数据文件
    cStream.open("AircraftSimulationData.txt");
    cStream.setf(ios::showpos);
    cStream.setf(ios::fixed);
    cStream.precision(8);

    // 仿真数据标题文本
    cStream << "Time(s)"
            << "\t";
    cStream << "Longitude(deg)"
            << "\t"
            << "Latitude(deg)"
            << "\t"
            << "Altitude(m)"
            << "\t";
    cStream << "V-North(m/s)"
            << "\t"
            << "V-Up(m/s)"
            << "\t"
            << "V-East(m/s)"
            << "\t";
    cStream << "Gamma(deg)"
            << "\t"
            << "Psi(deg)"
            << "\t"
            << "Zeta(deg)"
            << "\t";
    cStream << "OmegaX(deg/s)"
            << "\t"
            << "OmegaY(deg/s)"
            << "\t"
            << "OmegaZ(deg/s)"
            << "\t";
    cStream << "Fx-Gear(N)"
            << "\t"
            << "Fy-Gear(N)"
            << "\t"
            << "Fz-Gear(N)"
            << "\t";
    cStream << "Tx-Gear(N*m)"
            << "\t"
            << "Ty-Gear(N*m)"
            << "\t"
            << "Tz-Gear(N*m)"
            << "\t";
    cStream << "Fx-Engine(N)"
            << "\t"
            << "Fy-Engine(N)"
            << "\t"
            << "Fz-Engine(N)"
            << "\t";
    cStream << "Tx-Engine(N*m)"
            << "\t"
            << "Ty-Engine(N*m)"
            << "\t"
            << "Tz-Engine(N*m)"
            << "\t";
    cStream << "Fx-Aero(N)"
            << "\t"
            << "Fy-Aero(N)"
            << "\t"
            << "Fz-Aero(N)"
            << "\t";
    cStream << "Tx-Aero(N*m)"
            << "\t"
            << "Ty-Aero(N*m)"
            << "\t"
            << "Tz-Aero(N*m)"
            << "\t";
    cStream << "Aileron(deg)"
            << "\t"
            << "Rudder(deg)"
            << "\t"
            << "Elevator(deg)"
            << "\t";
    cStream << "LeadingEdgeFlap(deg)"
            << "\t"
            << "TrailingEdgeFlap(deg)"
            << "\t"
            << "SpeedBrake(deg)"
            << "\t";
    cStream << "Mach(-)"
            << "\t"
            << "Alpha(deg)"
            << "\t"
            << "Beta(deg)"
            << "\t";
    cStream << endl;

    return true;
}

// 初始化函数
bool AircraftSimulation::initialize(string resourceDir)
{
    // 仿真时间记录
    currentTime = 0.0;
    currentCount = 0;

    // 系列模型初始化
    cDOFModel.initialize(stepSize);

    cAtmosModel.input.altitude = cDOFModel.parameter.LLA(2);
    cAtmosModel.initialize();

    cMassModel.input.fuelUsedL = 0.0;
    cMassModel.input.fuelUsedR = 0.0;
    cMassModel.initialize(AIRCRAFT_F16);

    cAeroModel.initialize(AIRCRAFT_F16, resourceDir + "/F-16");

    cPilotModel.initialize();
    cControlModel.initialize(AIRCRAFT_F16, stepSize);

    cTurbineModel.initialize(ENGINE_F100_PW_229, resourceDir + "/F100-PW-229", stepSize);

    cLandingGearModel.initialize(AIRCRAFT_F16);

    cAileronActutor.initialize(stepSize);
    cRudderActutor.initialize(stepSize);
    cElevatorActutor.initialize(stepSize);
    cLeadingActutor.initialize(stepSize);
    cTrailingActutor.initialize(stepSize);
    cSpeedActutor.initialize(stepSize);

    return true;
}

// 单步运行函数
bool AircraftSimulation::step(void)
{
    double v = 0.0, mach = 0.0;
    double alpha = 0.0, beta = 0.0, q, g, a, b, t;

    while (currentTime < stopTime)
    {
        // 更新当前仿真时间
        currentTime = currentCount * stepSize;
        currentCount++;

        // 记录仿真数据
        cStream << currentTime << "\t";
        cStream << cDOFModel.output.LLA << cDOFModel.output.vNav << cDOFModel.output.eular << cDOFModel.output.omega;
        cStream << cLandingGearModel.output.force << cLandingGearModel.output.torque;
        cStream << cTurbineModel.output.force << cTurbineModel.output.torque;
        cStream << cAeroModel.output.force << cAeroModel.output.torque;
        cStream << cControlModel.output.F16.aileron << "\t" << cControlModel.output.F16.rudder << "\t" << cControlModel.output.F16.elevator << "\t";
        cStream << cControlModel.output.F16.leadingEdgeFlap << "\t" << cControlModel.output.F16.trailingEdgeFlap << "\t" << cControlModel.output.F16.speedBrake << "\t";
        cStream << mach << "\t" << alpha << "\t" << beta << "\t";
        cStream << cAileronActutor.output.y << "\t" << cRudderActutor.output.y << "\t" << cElevatorActutor.output.y << "\t";
        cStream << cLeadingActutor.output.y << "\t" << cTrailingActutor.output.y << "\t" << cSpeedActutor.output.y << "\t";
        cStream << endl;

        // 飞行运动学模型
        cDOFModel.input.force = cAeroModel.output.force + cLandingGearModel.output.force + cTurbineModel.output.force;
        cDOFModel.input.torque = cAeroModel.output.torque + cLandingGearModel.output.torque + cTurbineModel.output.torque;
        cDOFModel.input.mass = cMassModel.output.mass;
        cDOFModel.input.inertia = cMassModel.output.inertia;
        cDOFModel.input.g = Geodesy::gravity(cDOFModel.output.LLA(1), cDOFModel.output.LLA(2));
        cDOFModel.step();

        // 大气环境模型
        cAtmosModel.input.altitude = cDOFModel.output.LLA(2);
        cAtmosModel.step();

        // 飞行参数解算
        alpha = Coordinate::alpha(cDOFModel.output.vBody);
        beta = Coordinate::beta(cDOFModel.output.vBody);
        v = Vector::norm(cDOFModel.output.vBody);
        t = Geodesy::temperature(cDOFModel.output.LLA(2));
        mach = v / Geodesy::acousticVelocity(t);
        q = 0.5 * cAtmosModel.output.density * v * v;
        a = cAtmosModel.output.pressure / Geodesy::pressure(0.0);
        b = cAtmosModel.output.temperature / Geodesy::temperature(0.0);

        // 质量特性模型
        cMassModel.input.fuelUsedL = cTurbineModel.output.fuelUsed * 0.5;
        cMassModel.input.fuelUsedR = cTurbineModel.output.fuelUsed * 0.5;
        cMassModel.step();

        // 气动特性模型
        cAeroModel.input.F16.aileron = cAileronActutor.output.y;
        cAeroModel.input.alpha = alpha;
        cAeroModel.input.altitude = cDOFModel.output.LLA(2);
        cAeroModel.input.beta = beta;
        cAeroModel.input.CG = cMassModel.output.CG;
        cAeroModel.input.dynamicPressure = q;
        cAeroModel.input.F16.elevator = cElevatorActutor.output.y;
        cAeroModel.input.F16.gearPosition = cLandingGearModel.output.gearPosition;
        cAeroModel.input.F16.leadingEdgeFlap = cLeadingActutor.output.y;
        cAeroModel.input.mach = mach;
        cAeroModel.input.omega = cDOFModel.output.omega;
        cAeroModel.input.F16.rudder = cRudderActutor.output.y;
        cAeroModel.input.F16.speedBrake = cSpeedActutor.output.y;
        cAeroModel.input.F16.trailingEdgeFlap = cTrailingActutor.output.y;
        cAeroModel.input.velocity = v;
        cAeroModel.step();

        // 自动驾驶仪模型
        cPilotModel.input.time = currentTime;
        cPilotModel.input.LLA = cDOFModel.output.LLA;
        cPilotModel.input.vNav = cDOFModel.output.vNav;
        cPilotModel.input.eular = cDOFModel.output.eular;
        cPilotModel.input.omega = cDOFModel.output.omega;
        cPilotModel.input.groundReaction = cLandingGearModel.output.groundReactionN && cLandingGearModel.output.groundReactionL && cLandingGearModel.output.groundReactionR;
        cPilotModel.step();

        // 动力系统模型
        cTurbineModel.input.CG = cMassModel.output.CG;
        cTurbineModel.input.densityAltitude = FT_TO_M * 145426.0 * (1.0 - pow(a / b, 0.235));
        cTurbineModel.input.densityRatio = cAtmosModel.output.density / Geodesy::density(0.0);
        cTurbineModel.input.dynamicPressure = q;
        cTurbineModel.input.mach = mach;
        cTurbineModel.input.temperature = cAtmosModel.output.temperature;
        cTurbineModel.input.throttleCommand = cPilotModel.output.throttleCommand;
        cTurbineModel.step();

        // 起落架模型
        cLandingGearModel.input.altitude = cDOFModel.output.LLA(2);
        cLandingGearModel.input.brakeCommand = cPilotModel.output.brakeCommand;
        cLandingGearModel.input.CG = cMassModel.output.CG;
        cLandingGearModel.input.eular = cDOFModel.output.eular;
        cLandingGearModel.input.gearCommand = cPilotModel.output.gearCommand;
        cLandingGearModel.input.omega = cDOFModel.output.omega;
        cLandingGearModel.input.steerCommand = cPilotModel.output.steerCommand;
        cLandingGearModel.input.vBody = cDOFModel.output.vBody;
        cLandingGearModel.step();

        // 控制系统模型
        cControlModel.input.gLoad = cDOFModel.input.force * (1.0 / cMassModel.output.mass / G_EARTH);
        cControlModel.input.F16.aileronCommand = cPilotModel.output.aileronCommand;
        cControlModel.input.alpha = alpha;
        cControlModel.input.F16.elevatorCommand = cPilotModel.output.elevatorCommand;
        cControlModel.input.eular = cDOFModel.output.eular;
        cControlModel.input.F16.gearPosition = cLandingGearModel.output.gearPosition;
        cControlModel.input.F16.groundReaction = cLandingGearModel.output.groundReactionL && cLandingGearModel.output.groundReactionR;
        cControlModel.input.mach = mach;
        cControlModel.input.omega = cDOFModel.output.omega;
        cControlModel.input.F16.rudderCommand = cPilotModel.output.rudderCommand;
        cControlModel.input.F16.speedBrakeCommand = cPilotModel.output.speedBrakeCommand;
        cControlModel.input.velocity = v;
        cControlModel.step();

        // 位置伺服执行机构
        cAileronActutor.input.u = cControlModel.output.F16.aileron;
        cAileronActutor.step();

        cRudderActutor.input.u = cControlModel.output.F16.rudder;
        cRudderActutor.step();

        cElevatorActutor.input.u = cControlModel.output.F16.elevator;
        cElevatorActutor.step();

        cLeadingActutor.input.u = cControlModel.output.F16.leadingEdgeFlap;
        cLeadingActutor.step();

        cTrailingActutor.input.u = cControlModel.output.F16.trailingEdgeFlap;
        cTrailingActutor.step();

        cSpeedActutor.input.u = cControlModel.output.F16.speedBrake;
        cSpeedActutor.step();
    }

    cStream.close();

    return true;
}